#ifndef _TURNLR_
#define _TURNLR_
#include "HardwareInfo.c"
#include <GetSysTimeMs.h>
#include <SetLCD5Char.h>
#include <SetMotor_Code.h>
#include <GetMotorCode.h>
#include <SetMotorCode0.h>
#include <pid.h>
#include <SetInSound.h>

void TurnLR(int LR, int speed, double pules)
{
    unsigned int state = 0;
    unsigned long s = 0;
    int code = 0;
    long tmp = 0;
    long T1 = 0;
    long C1 = 0;
    double k = 8.8;
    k=8.8;
    int lsp=0;
    int rsp=0;
    
    if(pules>=0)
    {
    	if(LR==0)
    		lsp=speed;
    	else
    		rsp=speed;
    }
    else
    {
    	if(LR==0)
    		lsp=-speed;
    	else
    		rsp=-speed;
    }
    long m_T1=0;
    
    SetMotorCode0(1);
    SetMotorCode0(2);
    
    
    
    if(lsp == 0 && rsp == 0) 
         {  
            motorsWrite2(0, 0);
    		resetPID();
    		moving = 0;
            //break;
         }
        else moving = 1;
        
    resetPID();
    nextPID=GetSysTimeMs();
    m_T1=GetSysTimeMs();
    m_lsp=lsp;
    m_rsp=rsp;
    leftPID.TargetTicksPerFrame = lsp;
    rightPID.TargetTicksPerFrame = rsp;
    while(1)
    {
    
    	if (GetSysTimeMs()>=nextPID) {
    		updatePID();
    		nextPID += 68;
    	  }
    
    		if ( fabs(GetMotorCode(1)) >=fabs(pules*k) || fabs(GetMotorCode(2)) >=fabs(pules*k) || GetSysTimeMs() -m_T1>fabs(pules)*800/speed+1000) {
    		
    		if(fabs(GetMotorCode(1)) <fabs(pules*k) && fabs(GetMotorCode(2)) <fabs(pules*k) )
    			SetInSound(840, 1000);
    		
    		motorsWrite2(0, 0);
    		moving = 0;
    		break;
      }
    /*
    SetLCD5Char(0,  120, GetSysTimeMs() , YELLOW, BLACK);
    SetLCD5Char(55, 80, GetMotorCode(2), YELLOW, BLACK);
    
    SetLCD5Char(0, 0, readPidOut(1)/2.55, YELLOW, BLACK);
    
    SetLCD5Char(0, 32, readPidOut(2)/2.55, YELLOW, BLACK);
    
    SetLCD5Char(55, 0, readPidIn(1), YELLOW, BLACK);
    
    SetLCD5Char(55, 32, readPidIn(2), YELLOW, BLACK);
    */
    }
    //SetLCD5Char(0,  120, GetSysTimeMs() , YELLOW, BLACK);
    
}
#endif

